#include <ros/ros.h>
#include <turtlesim/Spawn.h>
#include <geometry_msgs/Twist.h>
#include <tf/transform_listener.h>

int main(int arg, char** args)
{
    ros::init(arg, args, "my_tf_listener");

    ros::NodeHandle node;

    ros::service::waitForService("/spawn");
    ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("/spawn");
    turtlesim::Spawn srv;
    add_turtle.call(srv);

    ros::Publisher turtle_pub = node.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel", 100);

    tf::TransformListener listener;

    ros::Rate loop_rate(100);

    while (node.ok())
    {
        tf::StampedTransform transform;
        try
        {
            listener.waitForTransform("/trutle2", "/turtle1", ros::Time(0), ros::Duration(0.1));
            listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);
        }
        catch(tf::TransformException &ex)
        {
            ROS_ERROR("%s", ex.what());
            ros::Duration(1.0).sleep();
            continue;
        }

        geometry_msgs::Twist cmd_vel;
        cmd_vel.angular.z = 4.0 * atan2(transform.getOrigin().y(), transform.getOrigin().x());
        cmd_vel.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) + pow(transform.getOrigin().y(), 2));
        turtle_pub.publish(cmd_vel);

        loop_rate.sleep();
    }
    

    return 0;
}